
clc,clear; close all;
rMinHistory = [];
rMaxHistory = [];
sim_len = 1000;
num = 100; % number of points to draw colormap
C = rand(3, 2).* 5;
%C = [0.4 2; 4.9 3];
C_error = zeros(1, size(C, 1));

metricsHistory = zeros(num, num, size(C, 1));
rHistory = [];
S_estHistory = zeros(1, sim_len);
B_estHistory = zeros(1, sim_len);
C_estHistory = zeros(size(C, 1), sim_len);
%%
for ii = 1:sim_len
    
    % initialization
    A = [0 0];
    B = [5 0];
%    subplot(3, 1, 1);
    xlim([-1 6]);
    ylim([-1 6]);

%    [xS, yS] = ginput(1)
    xS = rand * 6;
    yS = rand * 6;
    S = [xS yS];
    speed = 340;
    clc;
    
    % TDoAs
    percentage = 0.3;
    dt_AB = (getDistance(A, S) - getDistance(B, S))/speed * (1 + rand * 2 * percentage- percentage);
    dt_AC = ones(1, size(C, 1));
    for jj = 1:size(C, 1)
        dt_AC(jj) = (getDistance(A, S) - getDistance(C(jj, :), S))/speed * (1 + rand * 2 *percentage - percentage);
    end
    
    dt_BC = ones(1, size(C, 1));
    for jj = 1:size(C, 1)
        dt_BC(jj) = (getDistance(B, S) - getDistance(C(jj, :), S))/speed * (1 + rand * 2 * percentage - percentage);
    end
    
    
    % loudness
    lambda0 = 3;
    lambda = 3;
    
    K_variation = 1;
    KA = 1 * K_variation;
    KB = 1 * K_variation;
    KC = 1 * K_variation;
    
    
    PA = KA/getDistance(S, A)^lambda0;
    PB = KB/getDistance(S, B)^lambda0;
    
    PC = ones(1, size(C, 1));
    for jj = 1:size(C, 1)
        PC(jj) = KC/getDistance(S, C(jj, :))^lambda0;
    end
    
    den = 1 - (PA/PB)^(1/lambda);
    if den * dt_AB < 0
        warning den * dt_AB < 0;
        continue;
    end
    
    % use TDoAs and loudness to compute distances between sources and
    % devices
    SA_est = dt_AB * speed / den;
    SB_est = SA_est - dt_AB * speed;
    
    SA_est - getDistance(S, A)
    SB_est - getDistance(S, B)
    
    % inter-device estimation confident range
    rMin = abs(SA_est - SB_est);
    rMax = SA_est + SB_est;
    
    % confident range history
    rMinHistory = [rMinHistory rMin];
    rMaxHistory = [rMaxHistory rMax];
    r_samples = round(rMin * 100) / 100:0.01:round(rMax * 100) / 100;
    rHistory = [rHistory r_samples];
    % based on history, use a statistical algorithm to come up with a
    % single estimate
    max_rMin = max(rMinHistory)
    min_rMax = min(rMaxHistory)
    AB = mean([max(rMinHistory) min(rMaxHistory)]);
    AB_error = abs(AB - getDistance(A, B))
    
    [a, b, c] = mode(rHistory);
    mode_list = c{1};    
    mode_list_chosen = randperm(length(mode_list));
    AB2 = mode_list(mode_list_chosen(1));
    AB2_error = abs(AB2 - getDistance(A, B))
    
    B_estHistory(ii) = AB2_error;
    
    % get an estimate of B's location
    B_est = [AB2 0];
    
    % use particles to estimate S based on the location estimate of B
    particles = genParticles([-1 6], [-1 6], 2000);
    dist1 = abs(getDistance(particles, A) - SA_est);
    dist2 = abs(getDistance(particles, B_est) - SB_est);
    metrics = dist1 + dist2;
    [vindex, index] = sort(metrics, 'ascend');
    weights = 1 - ([1:length(index)]./length(index)).^2;
    
    % estimate S' location
    S_est = [particles(index(1), 1) particles(index(1), 2)];
    S_est(2) = abs(S_est(2));
    S_error = getDistance(S_est, S)
    S_estHistory(ii) = S_error;
    
    % estimate C' location
    for jj = 1:size(C, 1)
        xp = linspace(0, 5, num);
        yp = linspace(0, 5, num);
        [xx, yy] = meshgrid(xp, yp);
        SC_est = sqrt((xx - S_est(1)).^2 + (yy - S_est(2)).^2);
        dist1 = abs(SA_est - SC_est - dt_AC(jj) * speed);
        dist2 = abs(SB_est - SC_est - dt_BC(jj) * speed);
        metrics = dist1 + dist2;
        metricsHistory(:, :, jj) = metricsHistory(:, :, jj) + metrics;
        index = find(metricsHistory(:, :, jj) == min(min(metricsHistory(:, :, jj))));
        x_index = ceil(index(1)/num);
        y_index = mod(index(1), num);
        if y_index == 0
            y_index = num;
        end
        x_v = xp(x_index);
        y_v = yp(y_index);
        C_est = [x_v, y_v]
        C_error(jj) = getDistance(C_est, C(jj, :));
        C_estHistory(jj, ii) = C_error(jj);
    end
    C_error_max = max(abs(C_error))
    C_error_median = median(abs(C_error))
    % plot
    textd = 0.2;
    plot(A(1), A(2), '+', 'linewidth', 2);hold on;
    text(A(1) + textd, A(2) - textd, 'A');
    
    plot(B(1), B(2), 'r+', 'linewidth', 2);
    text(B(1) + textd, A(2) - textd, 'B');
    
    plot(S(1), S(2), 'c+', 'linewidth', 2);
    text(S(1) + textd, S(2) - textd, 'S');
    xlim([-1 11]);
    ylim([-1 11]);
    metrics = zeros(num, num);
    for jj = 1:size(C, 1)
        metrics = metricsHistory(:, :, jj) + metrics;
    end
    % surf(xx, yy, metrics./ii, 'EdgeColor', 'None');
    contour(xx, yy, metrics./ii, 100);
    for jj = 1:size(C, 1)
        plot(C(jj, 1), C(jj, 2), 'w+', 'linewidth', 2);
        text(C(jj, 1) + textd, C(jj, 2) - textd, 'C');
    end
    hold off;
%    subplot(3, 1, 2);hist(rHistory, 100);
%    subplot(3, 1, 3);
    plot(S_estHistory(1:ii)); hold on; plot(C_estHistory(:, 1:ii)', 'rx-'); 
    plot(B_estHistory(1:ii), 'c^-');hold off;
%    if ii >= 2 && B_estHistory(ii) > B_estHistory(ii - 1)
%        B_estHistory(ii)
%    end
    pause(0.8);
end